/**
 * @file Scene.h
 * @author 场景类 (you@domain.com)
 * @brief 
 * @version 0.1
 * @date 2024-03-13
 * 
 * @copyright Copyright (c) 2024
 * 
 */

#pragma once
#include <vector>
// #include <thread>
#include <boost/thread.hpp>
#include <ros/ros.h>
#include <dynamic_reconfigure/server.h>
#include <gazebo_msgs/SetLinkState.h>
#include <gazebo_msgs/SetLinkStateRequest.h>
#include <gazebo_msgs/GetLinkState.h>
#include <gazebo_msgs/JointRequest.h>
#include <geometry_msgs/PoseArray.h>

#include "UpdateObject.h"
#include "UGV.h"
#include <iusc_indoor_env/FormationConfig.h>

namespace iusc {
    class IUSCIndoorScene{
        public:
            void initialize(ros::NodeHandle& nh,ros::NodeHandle& pnh);
            void update(double dt);

            void formationCfgCallback(iusc_indoor_env::FormationConfig& config, uint32_t level);

        private:
            void loadConfig();  // 初始化ros参数 + 初始化对象
            void initializeObjects();  // 初始化各类对象

            void parseUGVFormationNodes();  // 解码UGV编队的参数配置
        
        private:
            // ros vars :
            ros::NodeHandle m_nh,m_pnh;
            ros::ServiceClient m_setLinkStateClient,m_getLinkStateClient;  // 设置状态的接口
            ros::Publisher m_refereeUGVPub;
            std::shared_ptr<dynamic_reconfigure::Server<iusc_indoor_env::FormationConfig>> m_formationServer;
            iusc_indoor_env::FormationConfig lastCfg;
            
            // objects :
            std::vector<UpdateObject::Ptr> m_updateObjects;  // 更新对象
            UGVFormation::Ptr m_formationObj;

            // params 
            bool m_cfgInitialized = false;
            UGVFormationParams m_formationParam;
            ros::Time m_lastPubStamp;
    };
}